package legway;
/**
 * This class is utilised to set and get common parameters between the Balance and
 * Behavioural threads. It is used to offset the motors power levels and the tilt offset of
 * the robot. Original code by Steven Witzand
 * 
 * @author Kim Petersen, Andreas Koefoed-Hansen og Tim Rasmussen
 * @version Januar 2010
 */
class CtrlParam
{
	public static final int STATIONARY = 0; // stop
	public static final int FORWARDS = 1; // forwards
	public static final int BACKWARDS = 2; // backwards

	private double offsetLeft = 0.0;
	private double offsetRight = 0.0;
	private static double  tiltAngle = 0.0;
	private int driveState = 0;
	private double damp = 0.0;
	private double maxForwardSpeed = 3;
	private double maxBackwardSpeed = 5;

	public void setMaxSpeed(double maxFowradSpeed, double maxBackwardSpeed) {
		this.maxForwardSpeed = maxFowradSpeed;
		this.maxBackwardSpeed = maxBackwardSpeed;
	}

	/**
	 * Run drive state is used to keep the Legway in a constant state of forwards or
	 * backwards motion. It can be set to keep the Legway stationary as well.
	 */
	public void runDriveState() {
		if (driveState == CtrlParam.FORWARDS)
			setTiltAngle(maxForwardSpeed - maxForwardSpeed * Math.exp(-damp));
		else if (driveState == CtrlParam.BACKWARDS)
			setTiltAngle(-maxBackwardSpeed + maxBackwardSpeed * Math.exp(-damp));
		else
			setTiltAngle(0);
		setDamp(0.1);
	}

	/**
	 * Set the Legway's LEFT motor offset.
	 * 
	 * @param offset A double used to set the left motor offset in degrees per second.
	 */
	public synchronized void setLeftMotorOffset(double offset) {
		this.offsetLeft = offset;
	}

	/**
	 * Set the Legway's RIGHT motor offset.
	 * 
	 * @param offset A double used to set the right motor offset in degrees per second.
	 */
	public synchronized void setRightMotorOffset(double offset) {
		this.offsetRight = offset;
	}

	/**
	 * Set the Legways LEFT motor offset.
	 * 
	 * @return A double of the left motor offset in degrees per second.
	 */
	public double getLeftMotorOffset() {
		return this.offsetLeft;
	}

	/**
	 * Set the Legways RIGHT motor offset.
	 * 
	 * @return A double of the right motor offset in degrees per second.
	 */
	public double getRightMotorOffset() {
		return this.offsetRight;
	}

	/**
	 * Set the tilt motor angle offset. The method works incrementally since it is the nature
	 * off the PID controller to remove the offset.
	 * 
	 * @param angle A double of the tilt angle offset in degrees.
	 */
	public synchronized void setTiltAngle(double angle) {
		this.tiltAngle += angle;
	}

	/**
	 * Get the tilt motor angle offset.
	 * 
	 * @return A double of the tilt angle offset in degrees.
	 */
	public static double getTiltAngle() {
		return tiltAngle;
	}

	/**
	 * Resets the tilt motor angle back to zero.
	 */
	public void resetTiltAngle() {
		this.tiltAngle = 0.0;
	}

	/**
	 * Set the current drive state of the Legway. 0 = Stop, 1 = Forwards, 2 = Backwards
	 * 
	 * @param state current drive state of the Legway
	 */
	public void setDriveState(int state) {
		this.driveState = state;
	}
	/**
	 * Returns the current drive state of the Legway
	 * 
	 * @return current drive state of the Legway
	 */
	public int getDriveState() {
		return this.driveState;
	}
	/**
	 * Testing methods used to slow the initial acceleration of the Legway
	 */
	public void setDamp(double weight) {
		this.damp += weight;
	}

	public void resetDamp() {
		this.damp = 0.0;
	}

	public double getDamp() {
		return this.damp;
	}
}